from machine import Pin
import time

# 电机引脚定义（可根据实际硬件修改）
MOTOR_A_DIR_PIN = 34
MOTOR_A_STP_PIN = 33
MOTOR_B_DIR_PIN = 46
MOTOR_B_STP_PIN = 35

# 步进电机参数配置（根据实际电机型号调整）
STEP_PER_180 = 100  # 电机转180度所需步数（常见200步/360度，故180度为100步）
SLOW_SPEED = 2500    # 慢速（延迟值越大速度越慢）
FAST_SPEED = 800     # 快速

# 电机控制类
class MotorControl:
    def __init__(self):
        # 初始化电机A引脚
        self.motor_a_dir = Pin(MOTOR_A_DIR_PIN, Pin.OUT)
        self.motor_a_stp = Pin(MOTOR_A_STP_PIN, Pin.OUT)

        # 初始化电机B引脚
        self.motor_b_dir = Pin(MOTOR_B_DIR_PIN, Pin.OUT)
        self.motor_b_stp = Pin(MOTOR_B_STP_PIN, Pin.OUT)

        # 电机状态初始化
        self.motors = {
            0: {'dir': 0, 'speed': SLOW_SPEED, 'step_count': 0},  # 电机A
            1: {'dir': 0, 'speed': SLOW_SPEED, 'step_count': 0}   # 电机B
        }

        # 初始电平设置
        self.motor_a_dir.value(0)
        self.motor_a_stp.value(0)
        self.motor_b_dir.value(0)
        self.motor_b_stp.value(0)

    def set_direction(self, motor_id, direction):
        """设置电机方向 0=正向 1=反向"""
        if motor_id not in (0, 1):
            return
        self.motors[motor_id]['dir'] = direction
        if motor_id == 0:
            self.motor_a_dir.value(direction)
        else:
            self.motor_b_dir.value(direction)

    def set_speed(self, motor_id, speed):
        """设置电机速度（延迟值，值越大速度越慢）"""
        if motor_id not in (0, 1):
            return
        if speed < 100:  # 限制最小延迟，避免速度过快烧毁电机
            speed = 100
        self.motors[motor_id]['speed'] = speed

    def rotate_angle(self, motor_id, angle, speed, direction=0):
        """
        电机旋转指定角度
        :param motor_id: 电机ID（0=A，1=B）
        :param angle: 旋转角度（度）
        :param speed: 旋转速度（延迟值）
        :param direction: 旋转方向（0=正向，1=反向）
        """
        if motor_id not in (0, 1):
            return

        # 计算所需步数
        steps = int(STEP_PER_180 * (angle / 180))
        self.set_direction(motor_id, direction)
        self.set_speed(motor_id, speed)

        # 执行旋转
        for _ in range(steps):
            if motor_id == 0:
                self.motor_a_stp.value(1)
                time.sleep_us(10)
                self.motor_a_stp.value(0)
            else:
                self.motor_b_stp.value(1)
                time.sleep_us(10)
                self.motor_b_stp.value(0)
            time.sleep_us(self.motors[motor_id]['speed'])

        # 更新步数计数
        self.motors[motor_id]['step_count'] += steps if direction == 0 else -steps

    def reset_to_origin(self, motor_id):
        """将电机复位到初始位置（反转相同步数）"""
        current_steps = self.motors[motor_id]['step_count']
        if current_steps == 0:
            return

        direction = 1 if current_steps > 0 else 0  # 反转方向
        steps = abs(current_steps)
        self.set_direction(motor_id, direction)
        self.set_speed(motor_id, self.motors[motor_id]['speed'])

        # 执行复位
        for _ in range(steps):
            if motor_id == 0:
                self.motor_a_stp.value(1)
                time.sleep_us(10)
                self.motor_a_stp.value(0)
            else:
                self.motor_b_stp.value(1)
                time.sleep_us(10)
                self.motor_b_stp.value(0)
            time.sleep_us(self.motors[motor_id]['speed'])

        self.motors[motor_id]['step_count'] = 0

# 初始化电机控制实例
motor_ctrl = MotorControl()

# 主循环：缓慢转动→复位→快速转动→复位→循环
def main_loop():
    print("开始电机循环控制...")
    while True:
        # ---------------------- 第一阶段：缓慢转动180度并复位 ----------------------
        print("=== 缓慢模式 ===")
        # A电机缓慢转180度（正向）
        print("A电机缓慢转180度...")
        motor_ctrl.rotate_angle(0, 180, SLOW_SPEED, direction=0)
        time.sleep(1)  # 停留1秒

        # B电机缓慢转180度（正向）
        print("B电机缓慢转180度...")
        motor_ctrl.rotate_angle(1, 180, SLOW_SPEED, direction=0)
        time.sleep(2)

        # 两个电机同时反转复位（回到原位）
        print("双电机复位...")
        motor_ctrl.reset_to_origin(0)
        motor_ctrl.reset_to_origin(1)
        time.sleep(2)

        # ---------------------- 第二阶段：快速转动180度并复位 ----------------------
        print("=== 快速模式 ===")
        # A电机快速转180度（正向）
        print("A电机快速转180度...")
        motor_ctrl.rotate_angle(0, 180, FAST_SPEED, direction=0)
        time.sleep(2)  # 快速模式停留时间缩短

        # B电机快速转180度（正向）
        print("B电机快速转180度...")
        motor_ctrl.rotate_angle(1, 180, FAST_SPEED, direction=0)
        time.sleep(2)

        # 两个电机同时反转复位（回到原位）
        print("双电机复位...")
        motor_ctrl.reset_to_origin(0)
        motor_ctrl.reset_to_origin(1)
        time.sleep(2)  # 两种模式切换间隔2秒

# 启动主循环
if __name__ == "__main__":
    try:
        main_loop()
    except KeyboardInterrupt:
        print("程序终止")
        # 紧急停止：复位所有电机到原位
        motor_ctrl.reset_to_origin(0)
        motor_ctrl.reset_to_origin(1)
